Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- component probe_sim "used for testing probe routines";
- pin in float x-in;
- pin in float y-in;
- pin in float z-in;
- pin in float probe-dia;
- pin out bit probe;
- param rw float probe-off-x=0.005;
- param rw float probe-off-y=-0.007;
- param rw float pin-x=5;
- param rw float pin-y=5;
- param rw float pin-d=2;
- param rw float pin-z=-1;
- param rw float rect-x=-5;
- param rw float rect-y=-5;
- param rw float rect-w=3;
- param rw float rect-h=2;
- param rw float rect-z=1;
- param rw float rect-offset=0.25;
- param rw float rect-slope=0;
- param rw float toolset-x=-5;
- param rw float toolset-y=5;
- param rw float toolset-d=0.5;
- param rw float toolset-z=-5;
- param rw float ring-x=5;
- param rw float ring-y=-5;
- param rw float ring-z=0;
- param rw float ring-d=3;
- param rw float ring-i=2;
- author "alex lait";
- license "GPL";
- function _;
- ;;
- #include "rtapi_math.h"
- FUNCTION(_){
- int probetrip = 0;
- int xtrip = 0;
- int ytrip = 0;
- //pin simulation
- float dist_pin = pow(pow(x_in - pin_x + probe_off_x, 2) + pow(y_in - pin_y + probe_off_y, 2), 0.5);
- if (( dist_pin <= (probe_dia / 2 + pin_d / 2)) && (z_in <= pin_z))
- {
- probetrip = 1;
- }
- //rectangle simulation
- float distx = fabs((x_in - rect_x + probe_off_x) + (y_in - rect_y + probe_off_y) * rect_slope);
- float disty = fabs((y_in - rect_y + probe_off_y) - (x_in - rect_x + probe_off_x) * rect_slope);
- if ((distx <= (probe_dia / 2 + rect_w / 2)) && (disty <= (probe_dia / 2 + rect_h / 2)) && (z_in <= rect_z))
- {
- if ((distx >= (rect_w / 2 - rect_offset - probe_dia / 2)) || (disty >= (rect_h / 2 - rect_offset - probe_dia / 2)))
- {
- probetrip = 1;
- }
- }
- //tool setter simulation
- dist_pin = pow(pow(x_in - toolset_x, 2) + pow(y_in - toolset_y, 2), 0.5);
- if ((dist_pin <= (probe_dia / 2 + toolset_d / 2)) && (z_in <= toolset_z))
- {
- probetrip = 1;
- }
- //ring simulation
- dist_pin = pow(pow(x_in - ring_x + probe_off_x, 2) + pow(y_in - ring_y + probe_off_y, 2), 0.5);
- if ((dist_pin <= (probe_dia / 2 + ring_d / 2)) && (dist_pin >= (ring_i / 2 - probe_dia / 2)) && (z_in <= ring_z))
- {
- probetrip = 1;
- }
- probe = probetrip;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement